/**
 * Delta X Firmware
 * Copyright (c) 2020 DeltaXFirmware [https://github.com/deltaxrobot/Delta-X-Firmware]
 *
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "config.h"
#include "Constants.h"

#include "fastio.h"

#include "Storage.h"
#include <ArduinoSTL.h>
#include <vector>
#include "GCodeReceiver.h"
#include "GCodeExecute.h"
#include "Planner.h"
#include "Motion.h"
#include "Tool.h"
#include "EndStops.h"
#include "EndEffector.h"
#include "MultiServo.h"
#include "Temperature.h"
#include "ConnectionState.h"

using namespace std;

GCodeReceiverClass GcodeReceiver;
GCodeExecuteClass GcodeExecute;

vector <String> GCodeQueue;
vector <Segment> SegmentQueue;

/**
 * 设置系统
 */
void setup() {
    SERIAL_PORT.begin(BAUDRATE); // 启动串口在 115200 波特率
    Data.init();    // 初始化常量参数
    Storage.init();  // EEPROM初始化

    DeltaKinematics.init(); // 初始化运动学模块，其中包括机器定义的参数、运动学正反解算法
    EndEffector.init(); // 末端执行器的初始化，包括各个末端控制的GPIO的工作模式及初始化值
    EndStops.init(); // 点击执行结束并停止的信号控制初始化
    Planner.init(&SegmentQueue); // 初始化执行计划管理器，包括设置执行段的vector数组表示的队列
    Stepper.init(&SegmentQueue); // 步进点击启动
    MultiServo.init(); // 多套私服器启动
    ConnectionState.Init(); // 连接状态初始化
    Motion.init(); // 运动指令管理器启动
    Temperature.init(); // 温度模块启动

    GcodeReceiver.Init(&GCodeQueue, &SERIAL_PORT, BAUDRATE); // G码接收器启动
    GcodeExecute.Init(&GCodeQueue); // G码执行器启动

    Serial.println("Init Success!");
    Motion.G28(); // 回零点
}

/**
 * 主循环
 */
void loop() {
    GcodeReceiver.Execute(); // G码接收
    GcodeExecute.Run();  // G码执行
    ConnectionState.Execute(); // 检查连接状态
    Temperature.ISR_EXECUTE(); // 温度检查
}
